
import rospy
from geometry_msgs.msg import Twist, Quaternion, Point
from nav_msgs.msg import Odometry
# from math import pi
from navigation import *
import time
from Control import *
import sys
sys.path.append('..')
# from cv_2022 import *
from ocr.detect_test import *
sys.path.insert(0,"/home/eaibot/2022_jsjds/ros_ws/devel/lib/python3.6/site-packages")
# sys.path.remove("/opt/ros/melodic/lib/python2.7/dist-packages")

import tf
from tf.transformations import euler_from_quaternion, quaternion_from_euler


def quat_to_angle(quat):
    rot = euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])
    # rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot[2]

class Control2():
    def __init__(self):
        self.tf_listener = tf.TransformListener()
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1) 
        self.rate = 50
        self.r = rospy.Rate(self.rate)
        # Set the odom frame
        self.odom_frame = '/odom_combined'
        self.base_frame = '/base_footprint'
        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")   

    def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

    def move(self,goal_distance,linear_speed=0.2):
        # Enter the loop to move along a side
        move_cmd = Twist() 
        move_cmd.linear.x = linear_speed 
        distance = 0
        (position, rotation) = self.get_odom()
        x_start = position.x
        y_start = position.y
        while distance < goal_distance and not rospy.is_shutdown():
            # Publish the Twist message and sleep 1 cycle         
            self.cmd_vel.publish(move_cmd)
            self.r.sleep()
            # Get the current position
            (position, rotation) = self.get_odom()
            # Compute the Euclidean distance from the start
            distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2))


if __name__ == '__main__':
    rospy.init_node("control2")
    co = Control2()
    while 1:
        print(co.get_odom())
        rospy.sleep(1)